
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       LowPassFilter2pFloat.c
  * @author     baiyang
  * @date       2021-11-7
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "LowPassFilter2p.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void DigitalBiquadFilterFloatCtor(DigitalBiquadFilterFloat *filter)
{
    filter->_delay_element_1 = 0.0f;
    filter->_delay_element_2 = 0.0f;
}

float DigitalBiquadFilterFloat_apply(DigitalBiquadFilterFloat *filter, float sample, const struct biquad_params *params)
{
    if(math_flt_zero(params->cutoff_freq) || math_flt_zero(params->sample_freq)) {
        return sample;
    }

    if (!filter->initialised) {
        DigitalBiquadFilterFloat_reset2(filter, sample, params);
        filter->initialised = true;
    }

    float delay_element_0 = sample - filter->_delay_element_1 * params->a1 - filter->_delay_element_2 * params->a2;
    float output = delay_element_0 * params->b0 + filter->_delay_element_1 * params->b1 + filter->_delay_element_2 * params->b2;

    filter->_delay_element_2 = filter->_delay_element_1;
    filter->_delay_element_1 = delay_element_0;

    return output;
}

void DigitalBiquadFilterFloat_reset(DigitalBiquadFilterFloat *filter)
{
    filter->_delay_element_1 = filter->_delay_element_2 = 0;
    filter->initialised = false;
}

void DigitalBiquadFilterFloat_reset2(DigitalBiquadFilterFloat *filter, float value, const struct biquad_params *params)
{
    filter->_delay_element_1 = filter->_delay_element_2 = value * (1.0 / (1 + params->a1 + params->a2));
    filter->initialised = true;
}

void DigitalBiquadFilterFloat_compute_params(DigitalBiquadFilterFloat *filter, float sample_freq, float cutoff_freq, struct biquad_params *ret)
{
    ret->cutoff_freq = cutoff_freq;
    ret->sample_freq = sample_freq;
    if (!math_flt_positive(ret->cutoff_freq)) {
        // zero cutoff means pass-thru
        return;
    }

    float fr = sample_freq/cutoff_freq;
    float ohm = tanf(M_PI/fr);
    float c = 1.0f+2.0f*cosf(M_PI/4.0f)*ohm + ohm*ohm;

    ret->b0 = ohm*ohm/c;
    ret->b1 = 2.0f*ret->b0;
    ret->b2 = ret->b0;
    ret->a1 = 2.0f*(ohm*ohm-1.0f)/c;
    ret->a2 = (1.0f-2.0f*cosf(M_PI/4.0f)*ohm+ohm*ohm)/c;
}

/*------------------------------------test------------------------------------*/


